package physics;

import java.util.Vector;
import physics.body.Shape;

/**
 * @see <a link href="http://chrishecker.com/Rigid_Body_Dynamics">Good stuff.</a>
 * @author peter
 */
public class Collision {
	static public boolean isCollision(Shape a, Shape b, float tdelta)
	{
		// insert quad devide here? yeah? yeah?


		// semi broad test
		if (boundingBoxTest(a.getBoundingBox(), b.getBoundingBox())) {

			PVector[] a_points = a.getPoints();
			PVector[] b_points = b.getPoints();
			PVector chead = new PVector();
			PVector ctail = new PVector();

			PVector tempCol;
			Vector<PVector> collisions = new Vector<PVector>();

			// check every edge for intersections.
			for (int i = 1; i < a_points.length; i++) {
				for (int j = 1; j < b_points.length; j++) {
					if ((tempCol = edgeIntersection(a_points[i - 1], a_points[i], b_points[j - 1], b_points[j], chead, ctail)) != null) {
						collisions.add(tempCol);
					}
				}
				if ((tempCol = edgeIntersection(a_points[i - 1], a_points[i], b_points[b_points.length - 1], b_points[0], chead, ctail)) != null) {
					collisions.add(tempCol);
				}
			}
			for (int j = 1; j < b_points.length; j++) {
				if ((tempCol = edgeIntersection(a_points[a_points.length - 1], a_points[0], b_points[j - 1], b_points[j], chead, ctail)) != null) {
					collisions.add(tempCol);
				}
			}

			if (collisions.size() > 0) { // there is a collision


				PVector cPoint = new PVector(collisions.get(0));
				PVector tail = new PVector(cPoint);


				// get avg collision pt
				for (int i = 1; i < collisions.size(); i++) {
					cPoint.add(collisions.get(i));
				}
				cPoint.scale(1.0 / (double) collisions.size());

				if (collisions.size() <= 2)
					tail = ctail;

				PVector head = new PVector(cPoint);

				PVector cNormal = head.getSub(tail).getPerp();

//				System.out.println("\ntail: " + tail.toString() + "\nhead: " + head.toString() + "\npos: " + a.getPosition().toString());
//				System.out.println("perp: " + perpendicularToCollision);
//				System.out.println("opperp: " + head.getSub(tail).getOpPerp().getSum(cPoint));
//				System.out.println("cPoint: " + cPoint);

				if (!isSameSide(
						a.getPosition(),
						cNormal.getSum(cPoint),
						tail,
						head))
					cNormal = cNormal.getNegative();
				a.collisionOccured();
				b.collisionOccured();

//				a.setNormalForce(cNormal.getSum(cPoint)); // for debugging only
//				a.setCollisionPoint(cPoint); //  ^
//				a.stepBack5();

				handleCollision(a, b, cPoint, cNormal, tdelta);
				return true;
			}
			return false;
		}
		return false;
	}

	static private boolean boundingBoxTest(PMatrix a, PMatrix b)
	{
		if (a.getCol1().getY() < b.getCol0().getY())
			return false;
		if (a.getCol0().getY() > b.getCol1().getY())
			return false;
		if (a.getCol1().getX() < b.getCol0().getX())
			return false;
		if (a.getCol0().getX() > b.getCol1().getX())
			return false;
		return true;
	}

	static private PVector edgeIntersection(PVector a_head, PVector a_tail, PVector b_head, PVector b_tail, PVector head, PVector tail)
	{
		// perpendicular to the face of b

		double det = determinant(a_tail.getSub(a_head), b_head.getSub(b_tail));
		if (det == 0)
			return null;
		double t = determinant(b_head.getSub(a_head), b_head.getSub(b_tail)) / det;
		double u = determinant(a_tail.getSub(a_head), b_head.getSub(a_head)) / det;
		if ((t < 0) || (u < 0) || (t > 1) || (u > 1))
			return null;

		head.set(b_head);
		tail.set(b_tail);

		return a_head.getScale(1 - t).getSum(a_tail.getScale(t));
	}


	// NEEDS IMPROVEMENT.... NEEDS TO RESOLVE ALL COLLISIONS AT ONCE
	static public void handleCollision(Shape a, Shape b, PVector position, PVector collisionNormal, float tdelta)
	{
		double e = a.getCoefficientOfRestitution();
		PVector perpRadiusA = position.getSub(a.getCenterOfMassLoc()).getPerp();
		PVector perpRadiusB = position.getSub(b.getCenterOfMassLoc()).getPerp();

		PVector velocityA = a.getVelocity().getSum(perpRadiusA.getScale(a.getAngularVelo()));
		PVector velocityB = b.getVelocity().getSum(perpRadiusB.getScale(b.getAngularVelo()));
		PVector velocityAB = velocityA.getSub(velocityB);

		double impulseNumerator = -(1.0 + e) * PVector.dotProduct(velocityAB, collisionNormal);

		double perpRadiusADotN = PVector.dotProduct(perpRadiusA, collisionNormal);
		double perpRadiusBDotN = PVector.dotProduct(perpRadiusB, collisionNormal);

		double impulseDenominator =
				PVector.dotProduct(collisionNormal, collisionNormal.getScale(a.getOneOverMass() + b.getOneOverMass())) + (perpRadiusADotN * perpRadiusADotN * a.getOneOverCMMomentOfInnertia()) + (perpRadiusBDotN * perpRadiusBDotN * b.getOneOverCMMomentOfInnertia());

		double impulse = impulseNumerator / impulseDenominator;

//		System.out.println("cn " + collisionNormal);
//		System.out.println("cmtcp " + perpRadiusA);
//		System.out.println("velo " + velocityAB);
//		System.out.println("oocmmofinn " + a.getOneOverCMMomentOfInnertia());
//		System.out.println("num = " + impulseNumerator);
//		System.out.println("perpDot = " + perpRadiusADotN);
//		System.out.println("denom = " + impulseDenominator);
//		System.out.println("impulse = " + impulse);
//		System.out.println();

		a.stepBack();
		b.stepBack();

		a.adjustVelocity(collisionNormal.getScale(impulse * a.getOneOverMass()));
		a.adjustAngularVelo(impulse * a.getOneOverCMMomentOfInnertia() * perpRadiusADotN);

		b.adjustVelocity(collisionNormal.getScale(-impulse * b.getOneOverMass()));
		b.adjustAngularVelo(-impulse * b.getOneOverCMMomentOfInnertia() * perpRadiusBDotN);// h@ll@

	}

	static private double determinant(PVector vec1, PVector vec2)
	{
		return vec1.getX() * vec2.getY() - vec1.getY() * vec2.getX();
	}

	static private boolean isNear(double valA, double valB)
	{
		return isNear(valA, valB, 0.0001);
	}

	static private boolean isNear(double valA, double valB, double diff)
	{
		return (valA + diff > valB && valA - diff < valB);
	}

	static private boolean isSameSide(PVector p1, PVector p2, PVector lineHead, PVector lineTail)
	{
		PVector headMTail = lineTail.getSub(lineHead);
		PVector cp1 = PVector.crossProduct(headMTail, p1.getSub(lineHead));
		PVector cp2 = PVector.crossProduct(headMTail, p2.getSub(lineHead));
		if (PVector.dotProduct(cp1, cp2) >= 0)
			return true;
		else
			return false;
	}
}
